"""
名称：CobotGPT_APF部署测试(虚拟机).py
功能：对人工势场法进行静态障碍物仿真，摄像头不参与，仅用来调试APF算法
缺陷：
"""

import queue
import timeit

import numpy as np
import torch
import torch.nn as nn

from Modules.APF.Modules.robot_class import Joint_point
from Modules.APF.User.Releases.Artifial_potential_field_RRT import APF_RRT, get_obstacle
from Modules.AUBO.User.Releases.robotcontrol import *


# from Modules.KINECT.User.Releases.kinectcontrol import *


def delay(microseconds):
    """
    功能：延迟xx微秒，1s = 1*10^3 ms = 1*10^6 us = 1*10^9 ns
    输入：
    输出
    """
    start = timeit.default_timer()
    while True:
        elapsed = (timeit.default_timer() - start) * 10 ** 6
        if elapsed >= microseconds:
            break
    return 0


class DNN(nn.Module):
    """
    * 功能：碰撞检测模型
    """

    def __init__(self):
        """ 搭建神经网络各层 """
        super(DNN, self).__init__()
        self.net = nn.Sequential(  # 按顺序搭建各层
            nn.Linear(36, 32), nn.Sigmoid(),  # 第 1 层：全连接层
            nn.Linear(32, 8), nn.Sigmoid(),  # 第 2 层：全连接层
            nn.Linear(8, 4), nn.Sigmoid(),  # 第 3 层：全连接层
            nn.Linear(4, 1), nn.Sigmoid()  # 第 4 层：全连接层
        )

    def forward(self, x):
        """ 前向传播 """
        y = self.net(x)  # x 即输入数据
        return y  # y 即输出数据


def Link_Transformation(last_i, i, a_list, alpha_list, d_list, theta_list):
    """
    * function：坐标系{i-1}到坐标系{i}的转换矩阵
    * tips：这里的last_i指的是i-1
    """
    i = i  # 下面使用的i-1表示列表的第i-1个数，注意同DH参数里的i-1区别
    T_martix = np.mat(np.zeros((4, 4)))

    T_martix[0, 0] = np.cos(theta_list[i - 1])
    T_martix[0, 1] = -1 * np.sin(theta_list[i - 1])
    T_martix[0, 2] = 0
    T_martix[0, 3] = a_list[i - 1]

    T_martix[1, 0] = np.sin(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 1] = np.cos(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 2] = -1 * np.sin(alpha_list[i - 1])
    T_martix[1, 3] = -1 * np.sin(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[2, 0] = np.sin(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 1] = np.cos(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 2] = np.cos(alpha_list[i - 1])
    T_martix[2, 3] = np.cos(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[3, 0] = 0
    T_martix[3, 1] = 0
    T_martix[3, 2] = 0
    T_martix[3, 3] = 1

    return T_martix


def get_robot_position(joint_radian: tuple = (0, 0, 0, 0, 0, 0)):
    """
    * 功能：机器人正解，获取六个关节坐标
    * 输入：机械臂关节角
    * 输出：六个关节坐标：6*3=18，类型tuple，单位m
    """
    # 初始化参数（DH参数）
    a_list = [0, 0, 408, 376, 0, 0]
    alpha_list = [0, np.pi / 2, np.pi, np.pi, np.pi / 2, np.pi / 2]
    d_list = [98.5, 121.5, 0, 0, 102.5, 94]
    # theta_list = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 输入想要转动的角度（此处设置转动的角度在转动后即达到机械臂伸直状态）
    theta_offset = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 关节角补偿
    # print(theta_offset)
    joint_radian = list(joint_radian)
    # print(joint_radian)
    theta_list = [a + b for a, b in zip(joint_radian, theta_offset)]  # 机械臂关节角：正转+，反转-
    # print(theta_list)

    # 例：T_0_1表示坐标系{0}到坐标系{1}的转换矩阵
    T_0_1 = Link_Transformation(0, 1, a_list, alpha_list, d_list, theta_list)
    T_1_2 = Link_Transformation(1, 2, a_list, alpha_list, d_list, theta_list)
    T_2_3 = Link_Transformation(2, 3, a_list, alpha_list, d_list, theta_list)
    T_3_4 = Link_Transformation(3, 4, a_list, alpha_list, d_list, theta_list)
    T_4_5 = Link_Transformation(4, 5, a_list, alpha_list, d_list, theta_list)
    T_5_6 = Link_Transformation(5, 6, a_list, alpha_list, d_list, theta_list)

    T_0_6 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6  # 二维数组，形状(4,4)，即矩阵
    T_0_5 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5
    T_0_4 = T_0_1 * T_1_2 * T_2_3 * T_3_4
    T_0_3 = T_0_1 * T_1_2 * T_2_3
    T_0_2 = T_0_1 * T_1_2
    T_0_1 = T_0_1
    # print(type(T_0_6))    # 查看数组形状：(4,4)
    # T_0_6 = np.array(T_0_6)
    # print(type(T_0_6))

    # 机器人关节坐标
    joint1_pos = T_0_1[0:3, 3].copy()
    joint2_pos = T_0_2[0:3, 3].copy()
    joint3_pos = T_0_3[0:3, 3].copy()
    joint4_pos = T_0_4[0:3, 3].copy()
    joint5_pos = T_0_5[0:3, 3].copy()
    joint6_pos = T_0_6[0:3, 3].copy()
    # print("joint1_pos: ",joint1_pos)
    # print("joint2_pos: ",joint2_pos)
    # print("joint3_pos: ",joint3_pos)
    # print("joint4_pos: ",joint4_pos)
    # print("joint5_pos: ",joint5_pos)
    # print("joint6_pos: ",joint6_pos)

    # <class 'numpy.matrix'> 转换为 <class 'numpy.ndarray'>
    arr1 = np.array(joint1_pos)
    arr2 = np.array(joint2_pos)
    arr3 = np.array(joint3_pos)
    arr4 = np.array(joint4_pos)
    arr5 = np.array(joint5_pos)
    arr6 = np.array(joint6_pos)
    # print(type(arr1))
    # print(arr6)
    # arr1 = arr1.astype(float)
    # arr2 = arr2.astype(float)
    # arr3 = arr3.astype(float)
    # arr4 = arr4.astype(float)
    # arr5 = arr5.astype(float)
    # arr6 = arr6.astype(float)

    # 使用flatten()方法将矩阵降级为一维数组
    flat_array_1 = arr1.flatten() / 1000.0
    flat_array_2 = arr2.flatten() / 1000.0
    flat_array_3 = arr3.flatten() / 1000.0
    flat_array_4 = arr4.flatten() / 1000.0
    flat_array_5 = arr5.flatten() / 1000.0
    flat_array_6 = arr6.flatten() / 1000.0
    # print(flat_array_1)
    # print(flat_array_2)
    # print(flat_array_3)
    # print(flat_array_4)
    # print(flat_array_5)
    # print(flat_array_6)

    # 使用tuple()函数将一维数组转换为元组
    # 改为list
    tuple1_arr = list(flat_array_1)
    tuple2_arr = list(flat_array_2)
    tuple3_arr = list(flat_array_3)
    tuple4_arr = list(flat_array_4)
    tuple5_arr = list(flat_array_5)
    tuple6_arr = list(flat_array_6)
    # print(tuple1_arr)
    # print(tuple2_arr)
    # print(tuple3_arr)
    # print(tuple4_arr)
    # print(tuple5_arr)
    # print(tuple6_arr)
    tuple_all = tuple1_arr + tuple2_arr + tuple3_arr + tuple4_arr + tuple5_arr + tuple6_arr
    # print(tuple_all)
    # print(type(tuple_all))

    joint_position = tuple_all
    # print(T_0_6)
    return joint_position


class CobotGPT:

    def __init__(self):
        """
        功能：连接机械臂
        输入：__init__在Class的不同方法里是不同步的，只能作为初始值使用，不能拿来做共同属性
        输出：连接状态码
        """

        # 链接服务器
        # self.ip = '192.168.213.128'  # 一楼
        self.ip = '192.168.199.128'  # 三楼，注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网
        self.port = 8899
        self.joint_radian_list = [(80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
                                  (
                                      160 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                      0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180,
                                   0 * pi / 180),
                                  (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),  # 第一阶段
                                  (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180),
                                  (90 * pi / 180, -20 * pi / 180, 80 * pi / 180, 10 * pi / 180, 90 * pi / 180,
                                   0 * pi / 180)]
        # length: 10
        self.robot_step = int()
        self.start = None
        self.goal = None

    def robot_set_position(self, joint_radian_queue: Queue, robot_step_queue: Queue):
        """
        * 功能：设置机械臂的固定轨迹点，并实时获取关节角信息
        * 输入：joint_radian_queue机械臂关节角(multiprocessing.Queue，maxsize=1)
        *       robot_step_queue机器人轨迹步数(multiprocessing.Queue，maxsize=1)
        * 输出：
        """

        # 初始化logger
        logger_init()
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("{0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                # robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))
                # robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))  # 低速挡
                # robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))  # 高速挡
                robot.set_joint_maxvelc((0.5, 0.5, 0.5, 0.5, 0.5, 0.5))  # 高速挡

                # 回到初始位姿(快速复位)
                joint_radian_ori = (0, 0, 0, 0, 0, 0)  # 初始位姿,turple类型
                logger.info("回到初始位姿：{0}".format(joint_radian_ori))
                robot.move_joint(joint_radian_ori)
                robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))  # 高速挡

                while True:
                    if joint_radian_queue.full() and robot_step_queue.full() is True:
                        try:
                            joint_radian = joint_radian_queue.get(timeout = 1)  # 从管道获取下一关节角
                            self.robot_step = robot_step_queue.get(timeout = 1)  # 从管道获取相应的机器人步数(多进程同步)
                            logger.info("[step {1}]: move joint to {0}".format(joint_radian, self.robot_step))
                            robot.move_joint(joint_radian)
                        except queue.Empty:
                            logger.info("TimeOut: queue is empty")



        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in robot_set_position()".format(robot.get_local_time(), e))

        # except KeyboardInterrupt:
        #     robot.move_stop()
        #     # 断开服务器链接
        #     if robot.connected:
        #         # 断开机械臂链接
        #         robot.disconnect()
        #     # 释放库资源
        #     Auboi5Robot.uninitialize()
        #     logger.info("KeyboardInterrupt: robot_set_position()")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} robot_set_position() is disconnected...".format(Auboi5Robot.get_local_time()))

    def collision_detection(self, joint_radian_queue: Queue, robot_step_queue: Queue):
        """
        * 功能：碰撞检测，并实时传输关节角给robot_set_position()
        * 输入：joint_radian_queue机械臂关节角(multiprocessing.Queue，maxsize=1)
        *       robot_step_queue机器人轨迹步数(multiprocessing.Queue，maxsize=1)
        * 输出：
        """

        # 初始化logger
        logger_init()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()

                # 加载离线碰撞检测模型(DNN)
                model_collision_detection = torch.load('model_collision_detection_v1.pth')  # 加载训练好的碰撞检测模型
                model_collision_detection.to('cuda:0')  # 模型加载到GPU上

                # 设定机器人的关节角空间
                joint_limit_angle = [
                    Joint_point(
                        [-170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180,
                         -170 * pi / 180]),
                    Joint_point([170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180,
                                 170 * pi / 180])]

                # 初始化机器人步数和碰撞标志
                self.robot_step = 0
                pred = 0

                while True:
                    # 碰撞检测：模型输入数据获取
                    # logger.info("========================================stop")
                    # robot.move_pause()

                    # 轨迹优化：模型输入数据获取，
                    # 组成：机器人关节坐标(6*3=18)，人体骨骼坐标(10*3=30)，当前关节角(6)，目标点关节角(6)
                    current_waypoint = robot.get_current_waypoint()
                    # print(f"current_waypoint: {current_waypoint}")
                    joint_radian = current_waypoint["joint"]  # 6列,tuple
                    # if self.robot_step == 2:
                    #     pred = 1
                    #     # 012碰撞
                    skeleton_data = [-0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                     -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                        , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                     0.22287961370105214, 0.3062320074838769,
                                     -0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                     -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                        , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                     0.22287961370105214, 0.3062320074838769,
                                     -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.466,
                                     0.399, 0.75]  # 30列,tuple
                    robot_position = get_robot_position(tuple(joint_radian))  # 18列
                    X = list(joint_radian + skeleton_data)
                    # print(f"X:{X}")

                    # 碰撞检测模块，用模型1
                    if len(X) == 36:
                        arr = np.array(X)  # 列表 => numpy数组
                        arr = arr.astype(np.float32)  # 转为 float32 类型numpy数组
                        ts = torch.tensor(arr)  # numpy数组 => 张量
                        ts = ts.to('cuda')
                        with torch.no_grad():  # 该局部关闭梯度计算功能
                            pred = model_collision_detection(ts)  # 一次前向传播（批量）
                            pred[pred >= 0.5] = 1
                            pred[pred < 0.5] = 0
                            pred = int(pred.item())  # 张量 => int值
                            # logger.info(f"是否碰撞: {pred}")
                    else:
                        # print("输入的总坐标格式错误")
                        pred = 0

                    # 碰撞标志处理
                    if pred == 0:
                        if joint_radian_queue.empty() and robot_step_queue.empty() is True:
                            try:
                                # logger.info("robot_step {0}".format(robot_step))
                                joint_radian_queue.put(self.joint_radian_list[self.robot_step], timeout = 1)
                                robot_step_queue.put(self.robot_step, timeout = 1)
                                self.robot_step += 1
                                if self.robot_step == 5:
                                    pred = 1
                                    self.robot_step = 2
                                if self.robot_step == int(len(self.joint_radian_list)):
                                    self.robot_step = 0
                            except queue.Full:
                                logger.info("TimeOut: queue is already full")

                    if pred == 1:
                        logger.info("检测到碰撞！！！！！！！！！！！！！！")
                        robot.move_pause()

                        skeleton_data = [-0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                         -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                            , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                         0.22287961370105214, 0.3062320074838769,
                                         -0.5043224874442438, 0.22287961370105214, 0.3062320074838769,
                                         -0.5043224874442438, 0.22287961370105214, 0.3062320074838769
                            , -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, -0.5043224874442438,
                                         0.22287961370105214, 0.3062320074838769,
                                         -0.5043224874442438, 0.22287961370105214, 0.3062320074838769, 0.24,
                                         0.44, 0.77]
                        robot_position = [0.0, -0.0, 0.0985, 0.09015006467853738, -0.0814568342034941,
                                          0.09850000000000002,
                                          0.13105307436421948, -0.03618857642294668, 0.5019126034671784,
                                          0.3715983664018288,
                                          0.23002817318868934, 0.38947326557888984, 0.4235396556924686,
                                          0.287512737066289,
                                          0.45658419898491714, 0.48124881149778836, 0.3155648722781763,
                                          0.38789117862410233]
                        # 获取的当前的关节坐标、骨骼坐标信息、当前关节角、目标关节角
                        skeleton_position_list = list(skeleton_data)  # 3*10=30，骨骼坐标
                        joint_position_list = list(robot_position)  # 3*6=18，关节坐标
                        # step_reset = self.robot_step - 2
                        # if step_reset < 0:
                        #     step_reset = 0
                        # start = list(self.joint_radian_list[step_reset])  # 上一步作为起点(关节角)
                        start = list(self.joint_radian_list[self.robot_step])  # 上一步作为起点(关节角)
                        goal = [160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180,
                                0 * pi / 180]  # 目标关节角
                        self.start = Joint_point(
                            [55 * pi / 180, -6 * pi / 180, 26 * pi / 180, 0 * pi / 180, 30 * pi / 180, -54 * pi / 180])
                        # self.start = Joint_point(start)
                        self.goal = Joint_point(goal)

                        # 先确定障碍物坐标，即找出距离机器人关节最近的骨骼点坐标，并将骨骼点坐标实例化为实体障碍物
                        cube_obstacle = get_obstacle(skeleton_position_list, joint_position_list)
                        # logger.info("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z,
                        #             cube_obstacle.l, cube_obstacle.w, cube_obstacle.h)
                        # logger.info("current joint radian:", joint_radian)
                        # logger.info("self.start:", self.start.theta)
                        # logger.info("self.goal:", self.goal.theta)
                        print("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z,
                              cube_obstacle.l, cube_obstacle.w, cube_obstacle.h)
                        print("current joint radian:", joint_radian)
                        print("self.start:", self.start.theta)
                        print("self.goal:", self.goal.theta)

                        # 轨迹优化：运行人工势场法
                        apf_rrt = APF_RRT(self.start, self.goal, cube_obstacle, joint_limit_angle)
                        start = time.time()
                        path = apf_rrt.path_planning()
                        end = time.time()
                        logger.info("轨迹优化完成, 耗费{:.2f}s".format(end - start))

                        # 运行轨迹优化路径点
                        path_list = list()
                        for i in range(len(path)):
                            path_list.append(path[i].theta)
                        # print("path_list:", path_list)

                        robot.move_continue()
                        self.robot_step = -1  # 表示轨迹优化的步数

                        while joint_radian_queue.empty() is not True:
                            joint_radian_queue.get()
                        while robot_step_queue.empty() is not True:
                            robot_step_queue.get()

                        for i in range(len(path_list)):
                            path = tuple(path_list[i])
                            # logger.info("[轨迹优化路径点 {1}]: {0}".format(path, i))

                            try:
                                joint_radian_queue.put(path, timeout = 10)
                                robot_step_queue.put(self.robot_step, timeout = 10)
                                logger.info("[轨迹优化路径点 {1}, step {2}]: {0}".format(path, i, self.robot_step))
                            except queue.Full:
                                logger.info("TimeOut: queue is already full")

                        self.robot_step = 0

                            # if joint_radian_queue.empty() and robot_step_queue.empty() is True:
                            #     try:
                            #         joint_radian_queue.put(path, timeout = 1)
                            #         robot_step_queue.put(self.robot_step, timeout = 1)
                            #         # logger.info("[轨迹优化路径点 {1}, step {2}]: {0}".format(path, i,self.robot_step))
                            #     except queue.Full:
                            #         logger.info("TimeOut: queue is already full")
                            # else:
                            #     while joint_radian_queue.empty() and robot_step_queue.empty() is not True:
                            #         # 循环等待两个管道清空
                            #         continue


        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in collision_detection()".format(robot.get_local_time(), e))

        # except KeyboardInterrupt:
        #     robot.move_stop()
        #     # 断开服务器链接
        #     if robot.connected:
        #         # 断开机械臂链接
        #         robot.disconnect()
        #     # 释放库资源
        #     Auboi5Robot.uninitialize()
        #     logger.info("KeyboardInterrupt: collision_detection()")

        # 无论是否有异常，都会执行的代码
        finally:
            logger.info("{0} collision_detection() is disconnected...".format(Auboi5Robot.get_local_time()))


if __name__ == '__main__':
    cobot = CobotGPT()

    # 创建通信队列
    q1 = Queue(maxsize = 1)  # joint_radian_queue
    q2 = Queue(maxsize = 1)  # robot_step_queue

    # 创建子进程
    p1 = Process(target = cobot.collision_detection,
                 kwargs = {"joint_radian_queue": q1, "robot_step_queue": q2})
    p1.start()

    # 主进程
    cobot.robot_set_position(joint_radian_queue = q1, robot_step_queue = q2)
